#!/usr/bin/env python3

import rospy
import math
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from tf.transformations import euler_from_quaternion
from std_msgs.msg import String

class NavToPoint:
    """改进的点对点直接导航控制器"""
    
    def __init__(self):
        rospy.on_shutdown(self.clean_up)
        
        # 创建cmd_vel发布者，直接控制机器人移动
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        
        # 订阅里程计信息，获取机器人当前位置
        self.pose_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        
        # 订阅激光扫描数据，用于避障
        self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
        
        # 状态发布器
        self.status_pub = rospy.Publisher('/nav_to_point/status', String, queue_size=5)
        
        # 初始化机器人位置和姿态
        self.current_x = 0.0
        self.current_y = 0.0
        self.current_yaw = 0.0
        
        # 从参数服务器加载控制参数
        self.linear_speed = rospy.get_param('~linear_speed', 1.2)  # 降低默认线性速度
        self.angular_speed = rospy.get_param('~angular_speed', 0.8)  # 降低默认角速度
        self.position_tolerance = rospy.get_param('~position_tolerance', 0.08)  # 提高位置精度要求
        self.yaw_tolerance = rospy.get_param('~yaw_tolerance', 0.05)  # 提高角度精度要求
        self.goal_timeout = rospy.get_param('~goal_timeout', 120.0)  # 整体目标超时时间
        self.stop_timeout = rospy.get_param('~stop_timeout', 1.0)  # 停止超时时间
        self.auto_stop_on_goal = rospy.get_param('~auto_stop_on_goal', True)  # 到达目标自动停止
        
        # 避障参数
        self.obstacle_detected = False
        self.min_front_distance = float('inf')
        self.obstacle_threshold = 0.1 # 障碍物阈值，单位米
        
        # 动作控制参数
        self.rate = rospy.Rate(25)  # 降至10Hz更稳定
        self.is_running = False
        self.target_reached = False
        
        rospy.loginfo('直接控制导航系统准备就绪')
    
    def odom_callback(self, msg):
        """处理里程计信息，更新机器人当前位置"""
        position = msg.pose.pose.position
        self.current_x = position.x
        self.current_y = position.y
        
        # 从四元数中提取yaw角度
        orientation = msg.pose.pose.orientation
        _, _, self.current_yaw = euler_from_quaternion(
            [orientation.x, orientation.y, orientation.z, orientation.w])
    
    def scan_callback(self, msg):
        """处理激光扫描数据，用于避障"""
        # 提取前方90度范围内的障碍物距离
        if not msg.ranges:
            return
            
        # 确定正前方的扫描范围
        total_points = len(msg.ranges)
        center_index = total_points // 2
        angle_span = 90  # 检查前方90度范围的障碍物
        start_index = center_index - angle_span // 2
        end_index = center_index + angle_span // 2
        
        # 获取前方有效距离值
        front_ranges = []
        for i in range(start_index, end_index):
            if i >= 0 and i < total_points:
                # 过滤掉无效值
                if msg.ranges[i] > 0.05 and not math.isinf(msg.ranges[i]) and not math.isnan(msg.ranges[i]):
                    front_ranges.append(msg.ranges[i])
        
        if front_ranges:
            self.min_front_distance = min(front_ranges)
            self.obstacle_detected = self.min_front_distance < self.obstacle_threshold
        else:
            self.min_front_distance = float('inf')
            self.obstacle_detected = False
    
    def normalize_angle(self, angle):
        """将角度规范化到 [-pi, pi] 范围内"""
        return math.atan2(math.sin(angle), math.cos(angle))
    
    def goto(self, target):
        """导航到目标点
        
        Args:
            target: [x, y, yaw_degrees] 列表，相对机器人的目标位置和朝向(角度)
              x=前进距离，y=侧向移动，yaw_degrees=旋转角度
        
        Returns:
            bool: 导航是否成功
        """
        if len(target) < 3:
            rospy.logerr("目标点格式错误，需要 [x, y, yaw_degrees]")
            return False
        
        # 清除之前的状态
        self.is_running = True
        self.target_reached = False
        
        # 提取目标点信息 - 注意这里使用的是相对坐标
        rel_x, rel_y = target[0], target[1]
        rel_yaw = math.radians(target[2])  # 将角度转换为弧度
        
        rospy.loginfo(f"开始导航到点: x={rel_x}, y={rel_y}, yaw={target[2]}°")
        
        # 计算目标在世界坐标系中的位置
        target_yaw = self.normalize_angle(self.current_yaw + rel_yaw)
        
        # 根据不同的导航命令选择不同的行为模式
        if abs(rel_x) < 0.01 and abs(rel_y) < 0.01 and abs(rel_yaw) > 0.1:
            # 纯旋转
            return self.rotate_to_yaw(target_yaw)
        elif abs(rel_x) > 0.01 and abs(rel_y) < 0.01 and abs(rel_yaw) < 0.1:
            # 纯直线运动
            return self.move_straight(rel_x)
        else:
            # 复合运动 - 先旋转，再移动，最后调整朝向
            # 1. 先旋转面向目标方向
            if abs(rel_yaw) > 0.1:
                self.rotate_to_yaw(target_yaw)
                
            # 2. 然后向前移动
            if abs(rel_x) > 0.01:
                self.move_straight(rel_x)
                
            # 3. 最后微调朝向
            if abs(rel_yaw) > 5:
                self.rotate_to_yaw(target_yaw)
            
            self.stop_robot()
            return True
    
    def move_straight(self, distance):
        """直线移动指定距离"""
        start_time = rospy.Time.now()
        timeout = rospy.Duration(min(30, abs(distance * 5)))  # 根据距离调整超时时间
        
        # 记录起始位置
        start_x = self.current_x
        start_y = self.current_y
        moved_distance = 0.0
        
        # 确定前进方向
        direction = 1 if distance > 0 else -1
        target_distance = abs(distance)
        
        while self.is_running and not rospy.is_shutdown():
            # 计算已移动的距离
            dx = self.current_x - start_x
            dy = self.current_y - start_y
            moved_distance = math.sqrt(dx*dx + dy*dy)
            
            # 判断是否到达目标距离
            if moved_distance >= target_distance:
                self.stop_robot()
                return True
            
            # 检测障碍物
            if self.obstacle_detected and direction > 0:  # 只有向前移动时才检查障碍物
                rospy.logwarn(f"检测到障碍物，距离: {self.min_front_distance:.2f}米")
                self.stop_robot()
                self.status_pub.publish("检测到障碍物，停止移动")
                return False
            
            # 计算剩余距离和适当的速度
            remaining = target_distance - moved_distance
            speed = min(self.linear_speed, max(0.05, remaining * 0.5))  # 根据剩余距离调整速度
            
            # 创建并发布速度命令
            twist = Twist()
            twist.linear.x = direction * speed
            self.cmd_vel_pub.publish(twist)
            
            # 检查是否超时
            if (rospy.Time.now() - start_time) > timeout:
                rospy.logerr("直线移动超时")
                self.stop_robot()
                return False
            
            self.rate.sleep()
        
        return False
    
    def rotate_to_yaw(self, target_yaw):
        """旋转到指定的绝对朝向"""
        start_time = rospy.Time.now()
        timeout = rospy.Duration(10)  # 减少超时时间
        
        # 记录初始角度差，用于判断进度
        initial_angle_diff = abs(self.normalize_angle(target_yaw - self.current_yaw))
        last_angle_diff = initial_angle_diff
        no_progress_start = rospy.Time.now()
        
        while self.is_running and not rospy.is_shutdown():
            # 计算需要旋转的角度
            angle_diff = self.normalize_angle(target_yaw - self.current_yaw)
            
            # 判断是否到达目标朝向
            if abs(angle_diff) < self.yaw_tolerance:
                self.stop_robot()
                rospy.loginfo(f"旋转完成，目标角度: {math.degrees(target_yaw):.2f}°")
                return True
            
            # 检测是否有实际旋转进度
            if abs(abs(angle_diff) - abs(last_angle_diff)) < 0.01:
                if (rospy.Time.now() - no_progress_start) > rospy.Duration(3):
                    rospy.logwarn("旋转无进度，放弃当前旋转任务")
                    self.stop_robot()
                    return False
            else:
                no_progress_start = rospy.Time.now()
                last_angle_diff = angle_diff
            
            # 更智能的速度控制 - 开始快，接近目标时减速
            progress = 1.0 - (abs(angle_diff) / max(0.1, initial_angle_diff))
            base_speed = self.angular_speed * (0.3 + 0.7 * (1.0 - progress))
            speed = min(base_speed, max(0.15, abs(angle_diff) * 0.5))
            direction = 1 if angle_diff > 0 else -1
            
            # 创建并发布速度命令
            twist = Twist()
            twist.angular.z = direction * speed
            self.cmd_vel_pub.publish(twist)
            
            # 检查是否超时
            if (rospy.Time.now() - start_time) > timeout:
                rospy.logwarn("旋转操作超时，停止当前导航")
                self.stop_robot()
                return False
            
            self.rate.sleep()
        
        return False

    def stop_robot(self):
        """安全地停止机器人移动"""
        twist = Twist()
        
        # 连续发送几次停止命令，确保机器人停止
        for _ in range(3):
            self.cmd_vel_pub.publish(twist)
            rospy.sleep(0.05)
        
    def clean_up(self):
        """清理资源，确保机器人停止"""
        self.is_running = False
        self.stop_robot()
        rospy.loginfo("导航节点已关闭，机器人已停止")

if __name__ == "__main__":
    try:
        rospy.init_node('nav_to_point')
        navigator = NavToPoint()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass